clc
clear
load LL

arfa=36.795/180*pi;
beta=78.169/180*pi;
%����Z��������תarfa�ĽǶȽ�������ת��XOZƽ����
T1=[cos(arfa)       -sin(arfa)         0         0
          sin(arfa)     cos(arfa)        0         0
                 0            0                    1         0
                0             0                     0        1];
%����Y��������ת90-beta�ĽǶȽ�������ת��Z����
T2=[cos(-pi/2+beta)      0   -sin(-pi/2+beta)      0 
                   0                 1                0                0
        sin(-pi/2+beta)       0     cos(-pi/2+beta)      0
                0                    0                    0             1];
  %��������ϵ�£�������Դ�տ������Ž������λ�ã�P���λ����Ȼ��Z��ĸ�������
[D1,S1 ]=xlsread('����1.csv','����1','A2:D2227'); %��ȡ��׼�������ڵ�����
[D2,S2]=xlsread('����2.csv','����2','A2:G2227'); %��ȡ�ٶ������¶˵��׼̬ʱ�˵�����
%[D3 S3]=xlsread('����3.csv','����3','A2:C4301'); %��ȡ������������ڵ���

%�������������ݱ任��������ϵ��
for i=1:length(D1)
   DD1(i,:)=[D1(i,:)       1]*T1*T2;    %���º�Ļ�׼�������ڵ�����
   DD2(i,:)=[D2(i,1:3)   1]*T1*T2;    %���º�Ĵٶ����¶˵��׼̬ʱ�˵�����
   DD3(i,:)=[D2(i,4:6)   1]*T1*T2;    %���º�Ĵٶ����϶˵��׼̬ʱ�˵�����
end

D1=DD1(:,1:3);
D2=[DD2(:,1:3)  DD3(:,1:3)];

dajja=zeros(40,1);
R=300.4;%��Բ�뾶 �ɼٶ�Ϊ300��300.4�������
%������������
tx=279.0:0.05:282;
ty=299.5:0.05:302;   
RD=0:150:150;  %���ݿھ���Χ
tic
for kk= 1:length(RD)-1
    kk
    clear ra  RLL NLL  rc d1 d2  
 for ii=1:length(tx)
     ii
      for jj=1:length(ty)
f=tx(ii);
c=ty(jj);

%%%%%%%%%%%%%%%%%%���ٶ������¶˵��������������潻��
M=D1(:,1)-D2(:,1);  %MNPΪ�ռ�ֱ�߱�׼�����м����
N=D1(:,2)-D2(:,2);
P=D1(:,3)-D2(:,3);
k=0; %�ھ�С�ڵ���300�׵ķ�Χ�������ڵ����
DD=zeros(size(D1)); %��ʼ����¼��Ӧ��i�������ڵ�仯���λ������
rc=[];
for i=1:length(D1(:,1))
    if (D1(i,1).^2+D1(i,2).^2).^0.5>=RD(kk)&(D1(i,1).^2+D1(i,2).^2).^0.5<=RD(kk+1)  %����ھ���ΧҪ��
        rc=[rc i];  %��¼��300�׿ھ��ڵĽڵ����
        k=k+1;
    x0=D2(i,1); %�ٶ����¶˵�
    y0=D2(i,2);
    z0=D2(i,3);
    m=M(i);  %����Ϊֱ�߷���xyz����ϵ��
    n=N(i);
    p=P(i);
    if m^2+n^2>0   %����ٶ�������ֱ��������������Ľ���
       t=qiugen(x0,y0,z0,m,n,p,f,c);
       zz(k)=min(z0+p*t); %ɸѡ��С��0��Z���꽻��
       t=(zz(k)-z0)/p;
       xx(k)=x0+m*t;
       yy(k)=y0+n*t;
       DD(i,:)=[xx(k) yy(k)  zz(k)];%��¼��Ӧ��i�������ڵ�仯���λ��
    else   %����ٶ�������ֱ����Z��ƽ��
       xx(k)=x0;
       yy(k)=y0;
        zz(k)=(x0.^2+y0.^2)/(2*f)-c;
      DD(i,:)=[xx(k) yy(k)  zz(k)];%��¼��Ӧ��i�������ڵ�仯���λ��
    end
    else
    DD(i,:)=[inf inf inf]; %����300�׿ھ���Χ�ڵ����¼Ϊ��Чֵ
   end
end
%%%%%%%%%%����ٶ�������ֱ��������������Ľ��� �� ��׼����ı仯����
for i=1:length(D1)
    distance(i)=(sum((DD(i,:)-D1(i,:)).^2))^0.5; %ֱ�Ӽ�����仯ǰ��ľ���
end
distance(find(isinf(distance)))=0;  %����Ч�����滻Ϊ0
NN(ii,jj,kk)=length(find(distance>0.6));  %��ʾ��������������Ƶ������ڵ����
ttt=0;
for i =1:100;
    ttt=ttt+1;
end
kkk=0;
for i =1:100;
    kkk=kkk+1;
end
ggg=0;
for i =1:100;
    ggg=ggg+1;
end




%���з���ģ��ʵ��
F=[0  0  f/2-c];
pn=0;
Rxy=[];
for i=1:length(rc) %���μ����i�������ڵ�仯���뷽Բ1��СԲ��Ľ���
    %����P���Z������ֵ��仯��ĵ�i�������ڵ��뽹������ֱ�߷���
    %P���Z������ֵ
    PZ=R*0.466-R; %��ֵ
    %�����ڵ��뽹�㷽������
    mx=DD(rc(i),1);
    ny=DD(rc(i),2);
    pz=DD(rc(i),3)-(f/2-c);
    t=(PZ-(f/2-c))/pz; %����P�����������ֱ�߲��������м����
    PX(i)=mx*t;       %�õ���i�������ڵ�仯���뷽Բ1��СԲ��Ľ���
    PY(i)=ny*t;
    %plot3([DD(i,1)  0],[DD(i,2)  0],[DD(i,3)  f/2-c],'b')  %���������ڵ��뽹������
    if (PX(i)^2+PY(i)^2).^0.5>0.5  %�ж������ڵ�仯���Ƿ��ڷ�Բ1��СԲ����
        pn=pn+1;
    end
      Rxy=[Rxy (PX(i)^2+PY(i)^2).^0.5]; %��¼�ڷ�Բ1��СԲ���ڽ���İ뾶
end
 MinRxy(ii,jj,kk)=max(Rxy);  %��¼��Ӧ��������µ����뾶
PN(ii,jj,kk)=pn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%���ɿھ������Ӿ���仯������
LF=zeros(size(LL));  %������ʼ�жϾ���
for i=1:length(LL)    %��Ӧ������
    for j=1:size(LL,2)   %��Ӧ����
          for k=1:length(rc)     %��Ӧ300�׿ھ��ڵĵ�k���ڵ�
               if  LL(i,j)==rc(k)     %�����k���ڵ������ھ���LL��i��j����ͬ����LFȡ1
                   LF(i,j)=1;
               end
          end
    end
end
lf=sum(LF') ;  %�����Ϊ2���ʾ�����Ӵ���
RLL=LL(find(lf==2),:); %300�׿ھ��ڵ��������ӹ�ϵ����
% n=length(RLL) ; %����Ч���ӽ��м���
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%�жϱ仯�ʵ����
for i=1:length(RLL)
     %�������ǰ��i�����ӵĳ���
     d1(i)=((D1(RLL(i,1),1)-D1(RLL(i,2),1))^2+(D1(RLL(i,1),2)-D1(RLL(i,2),2))^2+(D1(RLL(i,1),3)-D1(RLL(i,2),3))^2)^0.5;
     %����������i�����ӵĳ���
     d2(i)=((DD(RLL(i,1),1)-DD(RLL(i,2),1))^2+(DD(RLL(i,1),2)-DD(RLL(i,2),2))^2+(DD(RLL(i,1),3)-DD(RLL(i,2),3))^2)^0.5;
     %�������ǰ��仯��
     ra(i)=abs(d2(i)-d1(i))/d1(i);
end
nl(ii,jj,kk)=length(find(ra>0.0007));  %���������ӳ��ȱ仯Ҫ�������
NLL=RLL(find(ra>0.0007),:); %���������ӳ��ȱ仯�����Ӽ���
    end
 end
end
ttt=0;
for i =1:100;
    ttt=ttt+1;
end
kkk=0;
for i =1:100;
    kkk=kkk+1;
end
ggg=0;
for i =1:100;
    ggg=ggg+1;
end



for i=1:length(tx)
      for j=1:length(ty)
          F(i,j)=max([NN(i,j) PN(i,j) nl(i,j)]);  %�ҳ�ÿ������²��������������ڵ���
     end
end
[c  r]=find(F==min(min(F)))  %�ҳ����в���������������нڵ������ٵ����

% ���Ų������
txo=tx(c(1))  %����281.65
tyo=ty(r(1))   %���� 300.80


mm=zeros(20,1);
[D1,S1 ]=xlsread('����1.csv','����1','A2:D2227'); %��ȡ��׼�������ڵ�����
[D2,S2]=xlsread('����2.csv','����2','A2:G2227'); %��ȡ�ٶ������¶˵��׼̬ʱ�˵�����
[D3,S3]=xlsread('����3.csv','����3','A2:C4301'); %��ȡ������������ڵ���
plot3(D1(:,1),D1(:,2),D1(:,3),'b.')  %��׼�������ڵ�����
hold on
ffffff=ones(20,1);
for i=1:2226
      plot3([D2(i,1) D2(i,4) ],[D2(i,2) D2(i,5)],[D2(i,3) D2(i,6)],'g') %���ƴٶ������¶˵�����
      plot3([D2(i,4) D1(i,1) ],[D2(i,5) D1(i,2)],[D2(i,6) D1(i,3)],'y')  %�ٶ����϶˵㵽��׼�������ڵ�
end
XX=mean(D1(:,1));
YY=mean(D1(:,2));
ZZ=mean(D1(:,3));
X2=mean(D2(:,1));
Y2=mean(D2(:,2));
Z2=mean(D2(:,3));
X3=mean(D2(:,4));
Y3=mean(D2(:,5));
Z3=mean(D2(:,6));




clc
clear
load LL

arfa=36.795/180*pi;
beta=78.169/180*pi;
%����Z��������תarfa�ĽǶȽ�������ת��XOZƽ����
T1=[cos(arfa)       -sin(arfa)         0         0
          sin(arfa)     cos(arfa)        0         0
                 0            0                    1         0
                0             0                     0        1];
%����Y��������ת90-beta�ĽǶȽ�������ת��Z����
T2=[cos(-pi/2+beta)      0   -sin(-pi/2+beta)      0 
                   0                 1                0                0
        sin(-pi/2+beta)       0     cos(-pi/2+beta)      0
                0                    0                    0             1];
  %��������ϵ�£�������Դ�տ������Ž������λ�ã�P���λ����Ȼ��Z��ĸ�������
[D1,S1 ]=xlsread('����1.csv','����1','A2:D2227'); %��ȡ��׼�������ڵ�����
[D2,S2]=xlsread('����2.csv','����2','A2:G2227'); %��ȡ�ٶ������¶˵��׼̬ʱ�˵�����

%�������������ݱ任��������ϵ��
for i=1:length(D1)
   DD1(i,:)=[D1(i,:)       1]*T1*T2;    %���º�Ļ�׼�������ڵ�����
   DD2(i,:)=[D2(i,1:3)   1]*T1*T2;    %���º�Ĵٶ����¶˵��׼̬ʱ�˵�����
   DD3(i,:)=[D2(i,4:6)   1]*T1*T2;    %���º�Ĵٶ����϶˵��׼̬ʱ�˵�����
end

D1=DD1(:,1:3);
D2=[DD2(:,1:3)  DD3(:,1:3)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%������������һ�ķ����������
%���¾�Ϊ������ϵ�µļ������
plot3(D1(:,1),D1(:,2),D1(:,3),'b.')  %��׼�������ڵ�����
hold on
for i=1:2226
      plot3([D2(i,1) D2(i,4) ],[D2(i,2) D2(i,5)],[D2(i,3) D2(i,6)],'g') %���ƴٶ������¶˵�����
      plot3([D2(i,4) D1(i,1) ],[D2(i,5) D1(i,2)],[D2(i,6) D1(i,3)],'y')  %�ٶ����϶˵㵽��׼�������ڵ�
end
X=mean(D1(:,1));
Y=mean(D1(:,2));
Z=mean(D1(:,3));
X2=mean(D2(:,1));
Y2=mean(D2(:,2));
Z2=mean(D2(:,3));
X3=mean(D2(:,4));
Y3=mean(D2(:,5));
Z3=mean(D2(:,6));
plot3([0 X],[0 Y],[0 Z],'r')
%������������
R=300.4;
f=281.65;

c=300.8;
theta=0:0.01:2*pi;  %�����߼�����Ƕȷ�Χ
r=repmat(0:0.5:150,length(theta),1);   %�����߼�����뾶��Χ  ����Ϊ������ʽ
theta=repmat(theta',1,size(r,2));   %�����߼�����Ƕȷ�Χ ���µ����Ƕ�Ϊ������ʽ
x=r.*cos(theta);  %������ת��Ϊֱ����
y=r.*sin(theta);   %������ת��Ϊֱ����
z=(x.^2+y.^2)/(2*f)-c;  %���������߷���
surf(x,y,z)
shading interp
view([1 0 0])
axis equal
%%%%%%%%%%%%%%%%%%���ٶ������¶˵��������������潻��
M=D1(:,1)-D2(:,1);  %MNPΪ�ռ�ֱ�߱�׼�����м����
N=D1(:,2)-D2(:,2);
P=D1(:,3)-D2(:,3);
k=0; %�ھ�С�ڵ���300�׵ķ�Χ�������ڵ����
DD=zeros(size(D1)); %��ʼ����¼��Ӧ��i�������ڵ�仯���λ������
rc=[];
for i=1:length(D1(:,1))
    if (D1(i,1).^2+D1(i,2).^2).^0.5<=150&&(D1(i,1).^2+D1(i,2).^2).^0.5>=0  %����ھ�С�ڵ���300�׵ķ�ΧҪ��
        rc=[rc i];  %��¼��300�׿ھ��ڵĽڵ����
        k=k+1;
    x0=D2(i,1); %�ٶ����¶˵�
    y0=D2(i,2);
    z0=D2(i,3);
    m=M(i);  %����Ϊֱ�߷���xyz����ϵ��
    n=N(i);
    p=P(i);
    if m^2+n^2>0   %����ٶ�������ֱ��������������Ľ���
       t=qiugen(x0,y0,z0,m,n,p,f,c);
       zz(k)=min(z0+p*t); %ɸѡ��С��0��Z���꽻��
       t=(zz(k)-z0)/p;
       xx(k)=x0+m*t;
       yy(k)=y0+n*t;
       DD(i,:)=[xx(k) yy(k)  zz(k)];%��¼��Ӧ��i�������ڵ�仯���λ��
    else   %����ٶ�������ֱ����Z��ƽ��
       xx(k)=x0;
       yy(k)=y0;
        zz(k)=(x0.^2+y0.^2)/(2*f)-c;
      DD(i,:)=[xx(k) yy(k)  zz(k)];%��¼��Ӧ��i�������ڵ�仯���λ��
    end
    else
    DD(i,:)=[inf inf inf]; %����300�׿ھ���Χ�ڵ����¼Ϊ��Чֵ
   end
end
plot3(xx,yy,zz,'r.') %���Ƴ����дٶ�������ֱ��������������Ľ���
alpha(0.5)

%%%%%%%%%%����ٶ�������ֱ��������������Ľ��� �� ��׼����ı仯����
for i=1:length(D1)
    distance(i)=(sum((DD(i,:)-D1(i,:)).^2))^0.5; %ֱ�Ӽ�����仯ǰ��ľ���
end
distance(find(isinf(distance)))=0;  %����Ч�����滻Ϊ0
N=length(find(distance>0.6))  %��ʾ��������������Ƶ������ڵ����
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%�����ж�300�׿ھ��ڵ�

ttt=0;
for i =1:100;
    ttt=ttt+1;
end
kkk=0;
for i =1:100;
    kkk=kkk+1;
end
ggg=0;
for i =1:100;
    ggg=ggg+1;
end

F=[0  0  f/2-c];
for i=1:length(DD(:,1)) %���μ����i�������ڵ�仯���뷽Բ1��СԲ��Ľ���
    %����P���Z������ֵ��仯��ĵ�i�������ڵ��뽹������ֱ�߷���
    %P���Z������ֵ
    PZ=R*0.466-R; %��ֵ
    %�����ڵ��뽹�㷽������
    mx=DD(i,1);
    ny=DD(i,2);
    pz=DD(i,3)-(f/2-c);
    t=(PZ-(f/2-c))/pz; %����P�����������ֱ�߲��������м����
    PX(i)=mx*t;       %�õ���i�������ڵ�仯���뷽Բ1��СԲ��Ľ���
    PY(i)=ny*t;
    plot3([DD(i,1)  0],[DD(i,2)  0],[DD(i,3)  f/2-c],'b')  %���������ڵ��뽹������
end
figure(2) %���ƻ��������ڵ��뽹��������СԲ�潻��ֲ�ͼ
hold on
for  i=1:length(DD(:,1))
    plot3(PX(i),PY(i),PZ,'k.')  
end
theta=0:0.01:2*pi;  %P��СԲ�漫����Ƕȷ�Χ
r=repmat(0:0.05:1,length(theta),1);   %СԲ�漫����뾶��Χ  ����Ϊ������ʽ
theta=repmat(theta',1,size(r,2));   %СԲ��v������Ƕȷ�Χ ���µ����Ƕ�Ϊ������ʽ
x=r.*cos(theta);  %������ת��Ϊֱ����
y=r.*sin(theta);   %������ת��Ϊֱ����
z=PZ*ones(size(theta));  %СԲ��������
surf(x,y,z)
shading interp
alpha(0.5)
view([0  0  1])
save  rc  rc  DD
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%���Ӿ���仯������
load LL    %�������������ڵ����ӹ�ϵ����
LF=zeros(size(LL));  %������ʼ�жϾ���
for i=1:length(LL)    %��Ӧ������
    for j=1:size(LL,2)   %��Ӧ����
          for k=1:length(rc)     %��Ӧ300�׿ھ��ڵĵ�k���ڵ�
               if  LL(i,j)==rc(k)     %�����k���ڵ������ھ���LL��i��j����ͬ����LFȡ1
                   LF(i,j)=1;
               end
          end
    end
end
lf=sum(LF') ;  %�����Ϊ2���ʾ�����Ӵ���
RLL=LL(find(lf==2),:); %300�׿ھ��ڵ��������ӹ�ϵ����
n=length(RLL);  %����Ч���ӽ��м���



for i=1:length(RLL)
     %�������ǰ��i�����ӵĳ���
     d1(i)=((D1(RLL(i,1),1)-D1(RLL(i,2),1))^2+(D1(RLL(i,1),2)-D1(RLL(i,2),2))^2+(D1(RLL(i,1),3)-D1(RLL(i,2),3))^2)^0.5;
     %����������i�����ӵĳ���
     d2(i)=((DD(RLL(i,1),1)-DD(RLL(i,2),1))^2+(DD(RLL(i,1),2)-DD(RLL(i,2),2))^2+(DD(RLL(i,1),3)-DD(RLL(i,2),3))^2)^0.5;
     %�������ǰ��仯��
     ra(i)=abs(d2(i)-d1(i))/d1(i);
end

nl=length(find(ra>0.0007))  %���������ӳ��ȱ仯Ҫ�������
NLL=RLL(find(ra>0.0007),:); %���������ӳ��ȱ仯�����Ӽ���
%��ͼ�������ӹ�ϵ
figure
hold on
for i=1:length(RLL)    %����300�׿ھ��ڵ������߶�
     plot3([D1(RLL(i,1),1)   D1(RLL(i,2),1)],[D1(RLL(i,1),2)   D1(RLL(i,2),2)],[D1(RLL(i,1),3)   D1(RLL(i,2),3)],'b') 
end
for i=1:length(NLL)    %���Ʋ��������ӵ���Ҫ��������߶�
     plot3([D1(NLL(i,1),1)   D1(NLL(i,2),1)],[D1(NLL(i,1),2)   D1(NLL(i,2),2)],[D1(NLL(i,1),3)   D1(NLL(i,2),3)],'r') 
end
%%%%%%%%%%%%%%%%%%%%%%��任�õ����������涥������
Q=[0  0  -c 1]*inv(T1*T2)  %���Ϊ[-49.386  -36.939 -294.410]
for i=1:length(D1)
   DT1(i,:)=[DD(i,:)   1]*inv(T1*T2);    %��任��������ڵ�仯�������
end
DT=DT1(:,1:3);
%������任����ԭ����ϵ�е���ǰ�����ڵ�ı仯����
[D1 S1 ]=xlsread('����1.csv','����1','A2:D2227'); %��ȡ��׼�������ڵ�����
for i=1:length(DT)
    if isnan(DT(i,1))
        DT(i,:)=D1(i,:);
    end
    td(i)=norm(DT(i,:)-D1(i,:)); %����仯����
    if norm(DT(i,:))>norm(D1(i,:))
        td(i)=-td(i);
     end
end
xlswrite('DT.xls',round(DT*1000)/1000)
xlswrite('td.xls',round(td'*1000)/1000)










